/*
 *  matrixInvert.cpp
 *  
 *
 *  Based on matrixInvert.c, created by Joe Geigel on 4/23/08.
 *  Copyright 2008 __MyCompanyName__. All rights reserved.
 *
 *  Code derived from Ken Perlin's java Matrix class
 *
 */

#include "MatrixInvert.h"

/*
** Inverts the 4x4 matrix and stores the result in the object
** matrix.  
*/

void invert( float src[], float dst[] ) {
	
	// COMPUTE ADJOINT COFACTOR MATRIX FOR THE ROTATION+SCALE 3x3
	
	for (int i = 0; i < 3; i++) {
		for (int j = 0; j < 3; j++) {
			int i0 = (i + 1) % 3;
			int i1 = (i + 2) % 3;
			int j0 = (j + 1) % 3;
			int j1 = (j + 2) % 3;

			dst[(j << 2) + i] =
				src[(i0 << 2) + j0] * src[(i1 << 2) + j1] -
				src[(i0 << 2) + j1] * src[(i1 << 2) + j0];
		}
	}
			
	// RENORMALIZE BY DETERMINANT TO GET ROTATION+SCALE 3x3 INVERSE
			
	double determinant = src[(0 << 2) + 0] * dst[(0 << 2) + 0] +
			     src[(1 << 2) + 0] * dst[(0 << 2) + 1] +
			     src[(2 << 2) + 0] * dst[(0 << 2) + 2];

	double invd = 1.0 / determinant;

	for (int i = 0; i < 3; i++) {
		for (int j = 0; j < 3; j++) {
			dst[(i << 2) + j] *= invd;
		}
	}
	
	// COMPUTE INVERSE TRANSLATION
	
	for (int i = 0; i < 3; i++) {
		dst[(i << 2) + 3] = -dst[(i << 2) + 0] * src[(0 << 2) + 3] -
				     dst[(i << 2) + 1] * src[(1 << 2) + 3] -
				     dst[(i << 2) + 2] * src[(2 << 2) + 3];
	}

}
